package dai.math

import dai.core.base.Utils
import org.joml.AxisAngle4f
import org.joml.Quaternionf
import org.joml.Vector3f
import kotlin.math.cos
import kotlin.math.sin

fun main(){
    //
    val p = Vector3f(1f,0f,1f)

    /**
     * 问题: 求P点 绕  旋转轴  u = (0,1,0)旋转 90 度
     * 1. 将P点扩充到四元数, 即 P = (p,0)
     * 2. 而 q = (u * sin45,cos45)
     * 3. 利用公式 P' = q * p * q^-1
     */
//    val quatP = Quaternionf(p.x,p.y,p.z,0f)
//    val u = Vector3f(0f,1f,0f)
//
//    val rad = Utils.toRadius(90f)
//    val axisAngle = AxisAngle4f(rad,u)
//    val quatQ = Quaternionf(axisAngle)
//    val quatQInv = (quatQ.clone() as Quaternionf).invert()
//    val p1 = quatQ.mul(quatP).mul(quatQInv)
//    println("x:${p1.x},y:${p1.y},z:${p1.z},w:${p1.w}")


//    val p1 = Quaternionf(p.x,p.y,p.z,0f)
//    p1.normalize()
//    val euler = Vector3f()
//    p1.getEulerAnglesXYZ(euler)
//    println("euler:${Utils.toDegress(euler.y)}")
//    val dst = p1.clone() as Quaternionf
//    dst.rotateAxis(Utils.toRadius(90f),Vector3f(0f,1f,0f))
//    println("x:${dst.x},y:${dst.y},z:${dst.z},w:${dst.w}")
//    println(Math.fma(2f,3f,4f))

    val u = Vector3f(0f,2f,0f)
//    val p1 = Quaternionf().fromAxisAngleDeg(u,90f).transform(p)
//    println("x:${p1.x},y:${p1.y},z:${p1.z}")

     val p1 = Quaternionf().rotateAxis(Utils.toRadius(90f),u).transform(p)
    println("x:${p1.x},y:${p1.y},z:${p1.z}")
}


/**
 * p = q * p * q^-1
 * 求点P 绕 单位向量 normalizedAxis 旋转 degress 后的点 P1
 * srcPoint: 点P
 * normalizedAxis: 旋转轴 (单位向量)
 * degress: 要旋转的角度
 * @return 返回旋转后的点P1
 */
//fun rotate(srcPoint:Vector3f,normalizedAxis:Vector3f,degress:Float):Vector3f{
//
//}